import { Mesh } from '@babylonjs/core/Meshes/mesh';
import { Vector3, Matrix } from '@babylonjs/core/Maths/math.vector';
import { Face3 } from "../geometry/face3";
import { Utils } from "../tool/utils";
import { Calculation } from '../tool/calculation';
import { TransformNode } from '@babylonjs/core/Meshes/transformNode';
import { Geometry } from '@babylonjs/core/Meshes/geometry';
import { Scene } from '@babylonjs/core/scene';


/**
 * 预设随机表
 */
const RAND_LIST = new Int8Array([
    4, 6, -6, 5, 4, 2, -1, -3, 4, 8, 4, 7, -9, 6, 9, 8, 3, -1, 2, -1, -3, -2, -1, -5, -4, 8, 4, -7, 7, -8, 4, 7, 8,
    -3, -1, -5, -6, -1, 3, 1, 8, 4, -5, 1, 6, 4, 5, 4, 6, 1, 8, -9, 7, 9, 2, -7, 0, 3, -6, -5, -7, -5, 9, -5, 9, -1,
    9, 5, 3, 0, 9, 2, 1, 8, 6, -1, -1, 7, -3, 8, -1, 9, 3, -2, -6, 1, 1, 7, 9, -3, -1, -0, -5, -1, -1, -8, 5, 4, 8,
    -0, 7, 4, -4, -6, 2, 3, 7, -9, 9, -6, 2, 7, 4, 9, -5, 6, -7, -3, 5, 1, 8, 8, -5, 7, 5, 2, 7, -2, -4, 8, -9, -1,
    2, 2, -7, 9, -3, -8, -1, -8, -3, -0, -1, -1]
);

/**
 * 三角面的相等函数
 * @param a 三角面
 * @param b 三角面
 * @returns 是否相等
 */
const EQUAL = (a: Face3, b: Face3): boolean => {
    const na = a.normal;
    const nb = b.normal;
    const x = na.x - nb.x;
    const y = na.y - nb.y;
    const z = na.z - nb.z;
    return x * x + y * y + z * z < 1e-6;
};

/**
 * 边界面检测
 */
class FaceClipper {
    public mesh: Mesh;
    private _faces: Face3[] = [];
    private _cents: Vector3[] = [];
    private _ta: Vector3 = new Vector3();
    private _tb: Vector3 = new Vector3();
    private _tc: Vector3 = new Vector3();
    private _td: Vector3 = new Vector3();

    constructor(mesh: Mesh) {
        this.mesh = mesh;
        if (!mesh.geometry) console.warn('there is no [geometry]!', mesh);
    }

    /**
     * 创建三角面
     * @param a 顶点1
     * @param b 顶点2
     * @param c 顶点3
     */
    public createFace(a: Vector3, b: Vector3, c: Vector3): void {
        const face = new Face3(a, c, b);
        face.update();
        for (const f of this._faces) {
            if (EQUAL(f, face)) return;
        }
        this._cents.push(face.sphere.center);
        this._faces.push(face);
    }

    /**
     * 根据几何体生成计算使用的三角面
     */
    public initialize(): void {
        const g = this.mesh.geometry!;
        const pos = g.getVerticesData('position')!;
        const idx = g.getIndices();
        if (idx) {
            for (let i = 0; i < idx.length; i += 3) {
                const ai = idx[i] * 3, bi = idx[i + 1] * 3, ci = idx[i + 2] * 3;
                this._ta.set(pos[ai], pos[ai + 1], pos[ai + 2]);
                this._tb.set(pos[bi], pos[bi + 1], pos[bi + 2]);
                this._tc.set(pos[ci], pos[ci + 1], pos[ci + 2]);
                this.createFace(this._ta, this._tb, this._tc);
            }
        } else {
            for (let i = 0; i < pos.length; i += 9) {
                this._ta.set(pos[i], pos[i + 1], pos[i + 2]);
                this._tb.set(pos[i + 3], pos[i + 4], pos[i + 5]);
                this._tc.set(pos[i + 6], pos[i + 7], pos[i + 8]);
                this.createFace(this._ta, this._tb, this._tc);
            }
        }
    }

    /**
     * 检测质点与面的碰撞深度
     * @param mp 质点
     * @returns 深度
     */
    public checkPoint(mp: MassPoint): number {
        const p = this._ta.copyFrom(mp.position);
        const mg = mp.margin;
        let sus = 0, d = this._td;
        let maxDepth = 0;
        for (let i = 0; i < this._faces.length; i++) {
            const face = this._faces[i];
            const cent = this._cents[i];
            const n = face.normal;
            this._tb.copyFrom(n);
            this._tc.copyFrom(n).scaleInPlace(mg).addInPlace(p);
            this._tc.subtractInPlace(cent);
            if (Vector3.Dot(this._tb, this._tc) < 0) continue;
            const l = Calculation.projectOnVector(this._tc, n, this._tc).lengthSquared();
            if (maxDepth < l) maxDepth = l;
            d.copyFrom(n);
            sus++;
        }
        if (sus) {
            mp.count++;
            mp.lastNormal.copyFrom(d);
            mp.overStep++;
        }
        return Math.sqrt(maxDepth);
    }

    /**
     * 销毁
     */
    public dispose(): void {
        this._faces.length = 0;
        this._cents.length = 0;
    }
}

/**
 * 轴扫掠检测重叠对
 */
class SweepAxis {

    public pairs: Array<Array<number>> = [];
    public xPairs: Array<Array<number>> = [];
    public xyPairs: Array<Array<number>> = [];
    public xyzPairs: Array<Array<number>> = [];
    public tA0: Array<number> = [];
    public tA1: Array<number> = [];

    /**
     * 搜集重叠对（碰撞对）
     * @param ps 质点集合
     * @returns 重叠对（索引）
     */
    public collectOverlapPairs(ps: Array<MassPoint>): Array<number[]> {
        this.xyzPairs.length = 0;
        if (!ps.length) return this.xyzPairs;
        this.checkXAxis(ps);
        if (!this.xPairs.length) return this.xyzPairs;
        this.checkYAxis(ps);
        if (!this.xyPairs.length) return this.xyzPairs;
        this.checkZAxis(ps);
        return this.xyzPairs;
    }

    /**
     * 检测在z轴方向的重叠对
     * @param mps 质点集合
     */
    public checkZAxis(mps: Array<MassPoint>): void {
        for (let i = 0; i < this.xyPairs.length; i++) {
            const pa = mps[this.xyPairs[i][0]], pb = mps[this.xyPairs[i][1]];
            const az = pa.position.z, bz = pb.position.z;
            if (Math.abs(az - bz) < (pa.margin + pb.margin)) this.xyzPairs.push(this.xyPairs[i]);
        }
    }

    /**
     * 检测在y轴方向的重叠对
     * @param mps 质点集合
     */
    public checkYAxis(mps: Array<MassPoint>): void {
        this.xyPairs.length = 0;
        for (let i = 0; i < this.xPairs.length; i++) {
            let pa = mps[this.xPairs[i][0]], pb = mps[this.xPairs[i][1]];
            let ay = pa.position.y, by = pb.position.y;
            if (Math.abs(ay - by) < (pa.margin + pb.margin)) this.xyPairs.push(this.xPairs[i]);
        }
    }

    /**
     * 检测在x轴方向的重叠对
     * @param mps 质点集合
     */
    public checkXAxis(mps: Array<MassPoint>): void {
        this.xPairs.length = 0;
        const xs = this.tA0;
        xs.length = 0;
        for (let i = 0; i < mps.length; i++) {
            xs.push(mps[i].position.x);
        }
        const iAry = Utils.sortIndexArray(xs, undefined, this.tA1);
        for (let i = 0; i < iAry.length; i++) {
            for (let j = i + 1; j < iAry.length; j++) {
                const h = iAry[i], k = iAry[j];
                const cp = mps[h], np = mps[k];
                if (Math.abs(xs[h] - xs[k]) < (cp.margin + np.margin)) {
                    this.xPairs.push([h, k]);
                } else {
                    break;
                }
            }
        }
    }
}

/**
 * 质点可选参数对象
 * @param direction 运动方向
 * @param margin 体积范围
 */
interface IMassPointOptions {
    direction?: Vector3,
    margin?: number,
}

/**
 * 质点碰撞回调函数
 * @param targets 产生碰撞的对象集合
 */
interface IImpulseCallback {
    (targets: Array<MassPoint | Mesh>): void
}

/**
 * 质点
 */
class MassPoint {

    public count: number = 0;
    public lastNormal: Vector3 = new Vector3();
    public overStep: number = 0;
    public normal: Vector3 = new Vector3();

    private _object: TransformNode;
    private _container: Mesh;
    private _impulseCallback: IImpulseCallback | null = null;
    private _impulseTargets: Array<MassPoint | Mesh> = [];
    private _direction: Vector3 = new Vector3();
    private _margin: number = 0;
    private _position: Vector3 = new Vector3();
    private _tM0: Matrix = new Matrix();
    private _tV0: Vector3 = new Vector3(0, 0, 0);

    constructor(obj: TransformNode, container: Mesh, options?: IMassPointOptions) {
        this._object = obj;
        this._container = container;

        const ops = options || {};
        this._tM0.copyFrom(this._container.getWorldMatrix()).invert();
        this._direction.copyFrom(ops.direction || Vector3.UpReadOnly);
        this._margin = ops.margin || 0.01;

        Vector3.TransformCoordinatesToRef(this._tV0, this._object.getWorldMatrix(), this._tV0);
        Vector3.TransformCoordinatesToRef(this._tV0, this._tM0, this._tV0);
        this._position.copyFrom(this._tV0);
        this._tM0.getRotationMatrixToRef(this._tM0);
        Vector3.TransformCoordinatesToRef(this._direction, this._tM0, this._direction);
    }

    /**
     * 质点对象
     */
    public get object(): TransformNode {
        return this._object;
    }

    /**
     * 类型标识
     */
    public get type(): string {
        return 'massPoint';
    }

    /**
     * 碰撞回调函数
     * @param func_targets 回调函数
     */
    public set onImpulse(func_targets: IImpulseCallback | null) {
        this._impulseCallback = func_targets;
    }

    public get onImpulse(): IImpulseCallback | null {
        return this._impulseCallback;
    }

    /**
     * 获取碰撞对象
     */
    public get impulseTargets(): Array<MassPoint | Mesh> {
        return this._impulseTargets;
    }

    /**
     * 运动方向
     */
    public get direction(): Vector3 {
        return this._direction;
    }

    /**
     * 运动方向
     * @param vec 方向向量
     */
    public set direction(vec: Vector3) {
        this._direction.copyFrom(vec);
    }

    /**
     * 体积范围
     */
    public get margin(): number {
        return this._margin;
    }

    /**
     * 体积范围
     * @param num 范围值
     */
    public set margin(num: number) {
        this._margin = num;
    }

    /**
     * 位置
     */
    public get position(): Vector3 {
        return this._position;
    }

    /**
     * 位置
     * @param vec 位置
     */
    public set position(vec: Vector3) {
        this._position.copyFrom(vec);
        this.synchronize();
    }

    /**
     * 计算步进
     * @param dt 步进值 
     */
    public stepIn(dt: number): void {
        this._position.addInPlace(this._tV0.copyFrom(this._direction).scaleInPlace(dt));
    }

    /**
     * 与容器对象同步
     */
    public synchronize(): void {
        this._tV0.copyFrom(this._position);
        Vector3.TransformCoordinatesToRef(this._tV0, this._container.getWorldMatrix(), this._tV0);
        if (this._object.parent) {
            this._tM0.copyFrom(this._object.parent.getWorldMatrix()).invert();
            Vector3.TransformCoordinatesToRef(this._tV0, this._tM0, this._tV0);
        }
        this._object.position.copyFrom(this._tV0);
    }

    /**
     * 销毁
     */
    public dispose(): void {
        this._impulseTargets.length = 0;
    }
}

/**
 * 凸包质点容器可选参数对象
 * @param normalOffset 法线方向的偏移值
 * @param enableMassPointCollide 启用质点间的碰撞
 * @param worldSpace 是否是世界空间下
 */
interface IMassPointConvexContainerOptions {
    normalOffset?: number,
    enableMassPointCollide?: boolean,
    worldSpace?: boolean,
}

/**
 * 质点容器碰撞回调函数
 */
interface IMassPointContainerCallback {
    (container: Mesh, massPoints: MassPoint[], impulseMassPointIndices: number[]): void
}

/**
 * 凸包质点容器
 */
class MassPointConvexContainer {

    private _container: Mesh | null = null;
    private _points: Array<MassPoint> = [];
    private _faceClipper: FaceClipper | null = null;
    private _normalOffset: number;
    private _broadPhase: SweepAxis;
    private _tV0: Vector3 = new Vector3();
    private _containerCallback: IMassPointContainerCallback | null = null;
    private _impulseIndices: number[] = [];
    private _enableMassPointCollide: boolean;
    private _worldSpace: boolean;
    private _randT: number = 0;
    private _scene: Scene | null = null;

    constructor(container?: Mesh, scene?: Scene, options?: IMassPointConvexContainerOptions) {
        if (scene) this._scene = scene;
        const ops = options || {};
        this._normalOffset = ops.normalOffset || 0.2;
        this._broadPhase = new SweepAxis();
        this._enableMassPointCollide = ops.enableMassPointCollide || false;
        this._worldSpace = ops.worldSpace || false;

        this.container = container || null;
    }

    /**
     * 设置容器网格对象（凸包）
     */
    public set container(mesh: Mesh | null) {
        if (this._faceClipper) this._faceClipper.dispose();
        if (mesh && mesh.geometry) {
            mesh.computeWorldMatrix(true);
            if (!this._scene) this._scene = mesh.getScene();
            if (this._worldSpace) {
                if (this._container) this._container.geometry!.dispose();
                this._container = new Mesh('mass-point-convex-container-mesh', this._scene);
                const geo = Geometry.ExtractFromMesh(mesh, 'mass-point-convex-container-geometry' + Utils.id)!;
                geo.applyToMesh(this._container);
                this._container.bakeTransformIntoVertices(mesh.getWorldMatrix());
                this._container.visibility = 0;
            } else {
                this._container = mesh;
            }

            if (this._faceClipper) {
                this._faceClipper.mesh = this._container;
            } else {
                this._faceClipper = new FaceClipper(this._container);
            }
            this._faceClipper.initialize();
        } else {
            if (this._worldSpace) {
                if (this._container) this._container.geometry!.dispose();
            }
            this._container = null;
        }
    }

    /**
     * 获取容器网格对象
     */
    public get container(): Mesh | null {
        return this._container;
    }

    /**
     * 设置法线方向偏移值
     */
    public set normalOffset(num: number) {
        this._normalOffset = Math.max(num, 1e-2);
    }

    /**
     * 获取法线方向偏移值
     */
    public get normalOffset(): number {
        return this._normalOffset;
    }

    /**
     * 设置是否启用质点间的碰撞
     */
    public set enableMassPointCollide(enable: boolean) {
        this._enableMassPointCollide = enable;
    }

    /**
     * 获取质点间的碰撞是否被启用
     */
    public get enableMassPointCollide(): boolean {
        return this._enableMassPointCollide;
    }

    /**
     * 设置容器的碰撞回调函数
     */
    public set onContainerImpulse(func: IMassPointContainerCallback) {
        this._containerCallback = func;
    }

    /**
     * 添加质点
     * @param obj 质点对象
     * @param margin 体积范围
     * @param direction 运动方向
     * @returns 质点
     */
    public addMassPoint(obj: TransformNode, margin: number, direction: Vector3): MassPoint | null {
        if (!this._container) {
            console.warn('[.container] needs to be set before add any mass point!');
            return null;
        }
        const idx = this._points.findIndex(c => c.object.id === obj.id);
        if (idx < 0) {
            const mp = new MassPoint(obj, this._container, { margin: margin, direction: direction });
            this._points.push(mp);
            return mp;
        }
        console.warn('can only add one [mass point] for once!', obj);
        return null;
    }

    /**
     * 移除质点
     * @param obj 质点所绑定的对象
     */
    public removeMassPoint(obj: TransformNode): void {
        let idx = this._points.findIndex((c) => c.object.id === obj.id);
        if (idx < 0) return;
        this._points[idx].dispose();
        this._points.splice(idx, 1);
    }

    /**
     * 计算步进
     * @param dt 步进值 
     */
    public stepIn(dt: number): void {
        for (let mp of this._points) {
            mp.count = 0;
            mp.normal.set(0, 0, 0);
            mp.stepIn(dt);
            mp.impulseTargets.length = 0;
        }
        if (this._enableMassPointCollide) this.applyMassPointImpulse();
        this.applyContainerImpulse();
        for (let mp of this._points) {
            mp.synchronize();
            if (mp.onImpulse) {
                mp.onImpulse(mp.impulseTargets);
            }
        }
    }

    /**
     * 计算质点间的碰撞
     */
    private applyMassPointImpulse(): void {
        const pairs = this._broadPhase.collectOverlapPairs(this._points);
        const n = this._tV0;
        for (let i = 0; i < pairs.length; i++) {
            let pa = this._points[pairs[i][0]], pb = this._points[pairs[i][1]];
            let posA = pa.position, posB = pb.position;
            if (Vector3.DistanceSquared(posA, posB) < (pa.margin + pb.margin)) {
                n.copyFrom(posB).subtractInPlace(posA).normalize();
                pa.normal.addInPlace(n);
                pa.count++;
                pb.normal.subtractInPlace(n);
                pb.count++;
                pa.impulseTargets.push(pb);
                pb.impulseTargets.push(pa);
            }
        }
    }

    /**
     * 计算容器的碰撞
     */
    private applyContainerImpulse(): void {
        this._impulseIndices.length = 0;
        for (let i = 0; i < this._points.length; i++) {
            const mp = this._points[i];
            let dph = this._faceClipper!.checkPoint(mp);
            if (dph) {
                mp.impulseTargets.push(this._container!);
                this._impulseIndices.push(i);
            } else {
                mp.overStep = 0;
            }
            if (mp.count) {
                const n = dph ? mp.lastNormal : mp.normal.scaleInPlace(mp.count);
                if (n.x * n.x + n.y * n.y + n.z * n.z < 1e-6) {
                    n.set(this.rand(), this.rand(), this.rand()).normalize();
                } else {
                    n.normalize();
                }
                Vector3.ReflectToRef(mp.direction, n, mp.direction);
                if (dph) {
                    dph += mp.margin * this._normalOffset;
                    const dir = mp.direction;
                    const len = dir.length();
                    mp.position.addInPlace(n.scaleInPlace(-dph));
                    if (dir.x * dir.x + dir.y * dir.y + dir.z * dir.z < 1e-6) {
                        dir.set(this.rand(), this.rand(), this.rand());
                    }
                    dir.normalize().scaleInPlace(len);
                } else {
                    if (Vector3.Dot(mp.direction, n) > 0) {
                        mp.direction.negateInPlace();
                    }
                }
            }
        }
        if (this._containerCallback) this._containerCallback(this._container!, this._points, this._impulseIndices);
    }

    /**
     * 从随机表获取随机数
     * @returns 随机数 
     */
    private rand(): number {
        this._randT++;
        this._randT %= RAND_LIST.length;
        return RAND_LIST[this._randT];
    }
}


export { MassPointConvexContainer, MassPoint };
export type { IMassPointContainerCallback, IImpulseCallback, IMassPointConvexContainerOptions, IMassPointOptions };